#include "rtspserver3.h"

RtspServer3::RtspServer3(QObject *parent)
    : QObject{parent}
{
    m_server = NULL;
    m_socket = NULL;
    m_rtpSocket = NULL;
    m_rtcpSocket = NULL;
    m_bConnected = false;
    offsetPos = 0;

    h264File.setFileName(H264_FILE_NAME);
    accFile.setFileName(ACC_FILE_NAME);
}

RtspServer3::~RtspServer3()
{
    OnClosed();
    delete m_socket;
    delete m_server;
    m_rtpSocket = NULL;
    m_rtcpSocket = NULL;
    m_server = NULL;
    m_socket = NULL;
}

void RtspServer3::start()
{
    m_server = new QTcpServer(this);
    if(m_server) {
        bool ret = m_server->listen(QHostAddress::Any, SERVER_PORT);
        if(ret) {
            qDebug() << QString("服务器3监听端口:%1").arg(SERVER_PORT);
            connect(m_server, &QTcpServer::newConnection, this, &RtspServer3::OnNewConnection);
        }
    }
}

void RtspServer3::OnNewConnection()
{
    m_socket = m_server->nextPendingConnection();
    if(m_socket) {
        connect(m_socket, &QTcpSocket::readyRead, this, &RtspServer3::OnRecv);
        quint32 ip4 = m_socket->peerAddress().toIPv4Address();
        QHostAddress addrOrign = QHostAddress(ip4);

        clientIP = addrOrign.toString();
        clientPort = m_socket->peerPort();
    }
}

void RtspServer3::OnRecv()
{
    QString msg = m_socket->readAll();
    qDebug() << QString("msg:%1").arg(msg);
    OnMessage(msg);
}

void RtspServer3::OnMessage(QString recvMsg)
{
    QStringList msgList = recvMsg.split("\n");

    char method[40];
    char url[100];
    char version[40];
    int CSeq;

    int clientRtpPort, clientRtcpPort;
    for(int i = 0; i < msgList.size(); i++) {
        QString msg = msgList.at(i);
        if(msg.contains(OPTIONS) || msg.contains(DESCRIBE)
            || msg.contains(SETUP) || msg.contains(PLAY)) {
            if(sscanf(msg.toStdString().c_str(), "%s %s %s\r\n", method, url, version) != 3) {
                //error
            }
        } else if(msg.contains(CSEQ)) {
            if (sscanf(msg.toStdString().c_str(), "CSeq: %d\r\n", &CSeq) != 1) {
                //error
            }
        } else if(msg.contains(TRANSPORT)) {
            if (sscanf(msg.toStdString().c_str(), "Transport: RTP/AVP/UDP;unicast;client_port=%d-%d\r\n",
                       &clientRtpPort, &clientRtcpPort) != 2) {
                // error
            }
            qDebug() << QString("clientRtpPort:%1,clientRtcpPort:%2").arg(clientRtpPort).arg(clientRtcpPort);
        }
    }

    char* rBuf = (char*)malloc(BUF_MAX_SIZE);
    char* sBuf = (char*)malloc(BUF_MAX_SIZE);
    if (!strcmp(method, OPTIONS)) {
        if (handleCmd_OPTIONS(sBuf, CSeq)) {
            qDebug() << QString("failed to handle options");
        }
    } else if (!strcmp(method, DESCRIBE)) {
        if (handleCmd_DESCRIBE(sBuf, CSeq, url)) {
            qDebug() << QString("failed to handle describe");
        }
    } else if (!strcmp(method, SETUP)) {
        if (handleCmd_SETUP(sBuf, CSeq, clientRtpPort)) {
            qDebug() << QString("failed to handle setup\n");
        }
        OnCreatUdpSocket();
    } else if (!strcmp(method, PLAY)) {
        if (handleCmd_PLAY(sBuf, CSeq)) {
            qDebug() << QString("failed to handle play\n");
        }
    } else {
        printf("未定义的method = %s \n", method);
    }

    qDebug() << QString("<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
    qDebug() << QString("send: %1").arg(sBuf);
    m_socket->write(sBuf, strlen(sBuf));

    if (!strcmp(method, PLAY)) {
        int frameSize;
        uint8_t* frame = (uint8_t*)malloc(5000);

        if(!accFile.open(QIODevice::ReadOnly)) {
            qDebug() << QString("open file %1 error!").arg(accFile.fileName());
            return;
        }
        struct RtpPacket* rtpPacket = (struct RtpPacket*)malloc(5000);
        struct AdtsHeader adtsHeader;
        rtpHeaderInit(rtpPacket, 0, 0, 0, RTP_VESION, RTP_PAYLOAD_TYPE_AAC, 1, 0, 0, 0x32411);
        qDebug() << QString("start play acc.....");

        while(true) {
            frameSize = getFrameFromAccFile(frame, 7);///读取7Byte长度
            if (frameSize < 0){
                qDebug() << QString("读取%1结束,frameSize=%2").arg(ACC_FILE_NAME).arg(frameSize);
                break;
            }

            if (parseAdtsHeader(frame, &adtsHeader) < 0) {
                qDebug() << QString("parseAdtsHeader err");
                break;
            }

            frameSize = getFrameFromAccFile(frame, adtsHeader.aacFrameLength - 7);
            qDebug() << QString("frameSize:%1").arg(frameSize);
            if (frameSize < 0){
                qDebug() << QString("读取%1结束,frameSize=%2").arg(ACC_FILE_NAME).arg(frameSize);
                break;
            }

            //发送acc有效数据
            rtpSendAACFrame(rtpPacket, frame, adtsHeader.aacFrameLength - 7);

            QThread::msleep(1);
        }
        free(frame);
        free(rtpPacket);

        h264File.close();
    }
}

void RtspServer3::OnClosed()
{
    offsetPos = 0;
    if(m_rtpSocket) {
        m_rtpSocket->abort();
        m_rtpSocket->close();
    }
    if(m_rtcpSocket) {
        m_rtcpSocket->abort();
        m_rtcpSocket->close();
    }
    if(m_socket)
        m_socket->close();
    if(m_server)
        m_server->close();
}

int RtspServer3::OnCreatUdpSocket()
{
    m_rtpSocket = new QUdpSocket(this);
    if(m_rtpSocket->bind(SERVER_RTP_PORT)) {
        qDebug() << QString("bind rtp port: %1 success").arg(SERVER_RTP_PORT);
    }
    m_rtcpSocket = new QUdpSocket(this);
    if(m_rtcpSocket->bind(SERVER_RTCP_PORT)) {
        qDebug() << QString("bind rtcp port: %1 success").arg(SERVER_RTCP_PORT);
    }

    return 0;
}

int RtspServer3::handleCmd_OPTIONS(char* result, int cseq)
{
    sprintf(result, "RTSP/1.0 200 OK\r\n"
                    "CSeq: %d\r\n"
                    "Public: OPTIONS, DESCRIBE, SETUP, PLAY\r\n"
                    "\r\n",
            cseq);

    return 0;
}

int RtspServer3::handleCmd_DESCRIBE(char *result, int cseq, char *url)
{
    char sdp[500];
    char localIp[100];

    sscanf(url, "rtsp://%[^:]:", localIp);

    sprintf(sdp, "v=0\r\n"
                 "o=- 9%ld 1 IN IP4 %s\r\n"
                 "t=0 0\r\n"
                 "a=control:*\r\n"
                 "m=audio 0 RTP/AVP 97\r\n"
                 "a=rtpmap:97 mpeg4-generic/44100/2\r\n"
                 "a=fmtp:97 profile-level-id=1;mode=AAC-hbr;sizelength=13;indexlength=3;indexdeltalength=3;config=1210;\r\n"

                 //"a=fmtp:97 SizeLength=13;\r\n"
                 "a=control:track0\r\n",
            time(NULL), localIp);

    sprintf(result, "RTSP/1.0 200 OK\r\nCSeq: %d\r\n"
                    "Content-Base: %s\r\n"
                    "Content-type: application/sdp\r\n"
                    "Content-length: %d\r\n\r\n"
                    "%s",
            cseq,
            url,
            strlen(sdp),
            sdp);

    return 0;
}

int RtspServer3::handleCmd_SETUP(char *result, int cseq, int clientRtpPort)
{
    sprintf(result, "RTSP/1.0 200 OK\r\n"
                    "CSeq: %d\r\n"
                    "Transport: RTP/AVP;unicast;client_port=%d-%d;server_port=%d-%d\r\n"
                    "Session: 66334873\r\n"
                    "\r\n",
            cseq,
            clientRtpPort,
            clientRtpPort + 1,
            SERVER_RTP_PORT,
            SERVER_RTCP_PORT);

    return 0;
}

int RtspServer3::handleCmd_PLAY(char *result, int cseq)
{
    sprintf(result, "RTSP/1.0 200 OK\r\n"
                    "CSeq: %d\r\n"
                    "Range: npt=0.000-\r\n"
                    "Session: 66334873; timeout=10\r\n\r\n",
            cseq);

    return 0;
}

int RtspServer3::getFrameFromH264File(char *frame, int len)
{
    int readSize, frameSize;
    char* nextStartCode;

    readSize = h264File.read(frame, len);
    if (!startCode3(frame) && !startCode4(frame))
        return -1;

    nextStartCode = findNextStartCode(frame + 3, readSize - 3);
    if (!nextStartCode) {
        return -1;
    } else {
        frameSize = (nextStartCode - frame);

        offsetPos += frameSize;
        qDebug() << QString("current seek:%1, frameSize:%2").arg(offsetPos).arg(frameSize);
        h264File.seek(offsetPos);
    }

    return frameSize;
}

int RtspServer3::getFrameFromAccFile(uint8_t *frame, int len)
{
    return accFile.read((char *)frame, len);
}

int RtspServer3::parseAdtsHeader(uint8_t *frame, AdtsHeader *adtsHeader)
{
    memset(adtsHeader, 0, sizeof(*adtsHeader));

    if((frame[0] == 0xFF) && (frame[1] & 0xF0)) {
        adtsHeader->id = ((uint8_t)frame[1] & 0x08) >> 3;//第二个字节与0x08与运算之后，获得第13位bit对应的值
        adtsHeader->layer = ((uint8_t)frame[1] & 0x06) >> 1;//第二个字节与0x06与运算之后，右移1位，获得第14,15位两个bit对应的值
        adtsHeader->protectionAbsent = (uint8_t)frame[1] & 0x01;
        adtsHeader->profile = ((uint8_t)frame[2] & 0x0C) >> 6;
        adtsHeader->samplingFreqIndex = ((uint8_t)frame[2] & 0x3C) >> 2;
        adtsHeader->privateBit = ((uint8_t)frame[2] & 0x02) >> 1;
        adtsHeader->channelCfg = ((((uint8_t)frame[2] & 0x01) << 2) | (((unsigned int)frame[3] & 0xC0) >> 6));
        adtsHeader->originalCopy = ((uint8_t)frame[3] & 0x20) >> 5;
        adtsHeader->home = ((uint8_t)frame[3] & 0x10) >> 4;
        adtsHeader->copyrightIdentificationBit = ((uint8_t)frame[3] & 0x80) >> 3;
        adtsHeader->copyrightIdentificationStart = ((uint8_t)frame[3] & 0x40) >> 2;

        adtsHeader->aacFrameLength = (((((unsigned int)frame[3] & 0x30) << 11)
                                      | (((unsigned int)frame[4]) & 0xFF) << 3)
                                      | (((unsigned int)frame[5]) & 0xE0) >> 5);

        adtsHeader->adtsBufferFullness = ((((unsigned int)frame[5] & 0x1F) << 6)
                                          | ((unsigned int)frame[6] & 0xFC) >> 2);

        adtsHeader->numberOfRawDataBlockInFrame = ((uint8_t)frame[6]) & 0x03;

        return 0;
    } else {
        qDebug() << QString("failed to parse adts header");
        return -1;
    }
}

int RtspServer3::startCode3(char *buf)
{
    if (buf[0] == 0 && buf[1] == 0 && buf[2] == 1)
        return 1;
    else
        return 0;
}

int RtspServer3::startCode4(char *buf)
{
    if (buf[0] == 0 && buf[1] == 0 && buf[2] == 0 && buf[3] == 1)
        return 1;
    else
        return 0;
}

char *RtspServer3::findNextStartCode(char *buf, int len)
{
    int i;

    if (len < 3)
        return NULL;

    for (i = 0; i < len - 3; ++i) {
        if (startCode3(buf) || startCode4(buf))
            return buf;

        ++buf;
    }

    if (startCode3(buf))
        return buf;

    return NULL;
}

int RtspServer3::rtpSendH264Frame(RtpPacket *rtpPacket, char *frame, uint32_t frameSize)
{
    uint8_t naluType; // nalu第一个字节
    int sendBytes = 0;
    int ret;

    naluType = frame[0];
    if (frameSize <= RTP_MAX_PKT_SIZE) // nalu长度小于最大包长：单一NALU单元模式
    {

        //*   0 1 2 3 4 5 6 7 8 9
        //*  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
        //*  |F|NRI|  Type   | a single NAL unit ... |
        //*  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+

        memcpy(rtpPacket->payload, frame, frameSize);
//        ret = rtpSendPacketOverUdp(serverRtpSockfd, ip, port, rtpPacket, frameSize);
//        if(ret < 0)
//            return -1;

//        QByteArray sendDataStr;
//        sendDataStr.append((char *)rtpPacket, frameSize + RTP_HEADER_SIZE);
//        qDebug() << QString("单包发送:%1").arg(byteArrayToHexStr(sendDataStr));
        qDebug() << QString("单包发送字节数:%1, 开始字节:%2")
                        .arg(frameSize + RTP_HEADER_SIZE).arg(rtpPacket->payload[0], 8, 16, QLatin1Char('0'));
        ret = m_rtpSocket->writeDatagram((char *)rtpPacket, frameSize + RTP_HEADER_SIZE,
                                             QHostAddress(clientIP), clientPort);
        if(ret < 0)
            return -1;
        rtpPacket->rtpHeader.seq++;
        sendBytes += ret;
        if ((naluType & 0x1F) == 7 || (naluType & 0x1F) == 8) // 如果是SPS、PPS就不需要加时间戳
            goto out;
    }
    else // nalu长度小于最大包场：分片模式
    {

        //*  0                   1                   2
        //*  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3
        //* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
        //* | FU indicator  |   FU header   |   FU payload   ...  |
        //* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+



        //*     FU Indicator
        //*    0 1 2 3 4 5 6 7
        //*   +-+-+-+-+-+-+-+-+
        //*   |F|NRI|  Type   |
        //*   +---------------+



        //*      FU Header
        //*    0 1 2 3 4 5 6 7
        //*   +-+-+-+-+-+-+-+-+
        //*   |S|E|R|  Type   |
        //*   +---------------+


        int pktNum = frameSize / RTP_MAX_PKT_SIZE;       // 有几个完整的包
        int remainPktSize = frameSize % RTP_MAX_PKT_SIZE; // 剩余不完整包的大小
        int i, pos = 1;

        // 发送完整的包
        for (i = 0; i < pktNum; i++) {
            rtpPacket->payload[0] = (naluType & 0x60) | 28;
            rtpPacket->payload[1] = naluType & 0x1F;

            if (i == 0) //第一包数据
                rtpPacket->payload[1] |= 0x80; // start
            else if (remainPktSize == 0 && i == pktNum - 1) //最后一包数据
                rtpPacket->payload[1] |= 0x40; // end

            memcpy(rtpPacket->payload+2, frame+pos, RTP_MAX_PKT_SIZE);
//            ret = rtpSendPacketOverUdp(serverRtpSockfd, ip, port, rtpPacket, RTP_MAX_PKT_SIZE+2);
//            if(ret < 0)
//                return -1;

//            QByteArray sendDataStr;
//            sendDataStr.append((char *)rtpPacket, frameSize + RTP_HEADER_SIZE);
//            qDebug() << QString("分包发送:%1").arg(byteArrayToHexStr(sendDataStr));
            qDebug() << QString("分包发送字节数:%1, 开始字节:%2")
                            .arg(RTP_MAX_PKT_SIZE + 2 + RTP_HEADER_SIZE).arg(rtpPacket->payload[2], 8, 16, QLatin1Char('0'));
            int ret = m_rtpSocket->writeDatagram((char *)rtpPacket, RTP_MAX_PKT_SIZE + 2 + RTP_HEADER_SIZE,
                                                 QHostAddress(clientIP), clientPort);
            if(ret < 0)
                return -1;

            rtpPacket->rtpHeader.seq++;
            sendBytes += ret;
            pos += RTP_MAX_PKT_SIZE;
        }

        // 发送剩余的数据
        if (remainPktSize > 0)
        {
            rtpPacket->payload[0] = (naluType & 0x60) | 28;
            rtpPacket->payload[1] = naluType & 0x1F;
            rtpPacket->payload[1] |= 0x40; //end

            memcpy(rtpPacket->payload+2, frame+pos, remainPktSize+2);
//            ret = rtpSendPacketOverUdp(serverRtpSockfd, ip, port, rtpPacket, remainPktSize+2);
//            if(ret < 0)
//                return -1;
//            QByteArray sendDataStr;
//            sendDataStr.append((char *)rtpPacket, frameSize + RTP_HEADER_SIZE);
//            qDebug() << QString("剩余发送:%1").arg(byteArrayToHexStr(sendDataStr));
            qDebug() << QString("剩余发送字节数:%1, 开始字节:%2")
                            .arg(remainPktSize + 2 + RTP_HEADER_SIZE).arg(rtpPacket->payload[2], 8, 16, QLatin1Char('0'));
            int ret = m_rtpSocket->writeDatagram((char *)rtpPacket, remainPktSize + 2 + RTP_HEADER_SIZE,
                                                 QHostAddress(clientIP), clientPort);
            if(ret < 0)
                return -1;

            rtpPacket->rtpHeader.seq++;
            sendBytes += ret;
        }
    }
    rtpPacket->rtpHeader.timestamp += 90000 / 25;
out:

    return sendBytes;
}

int RtspServer3::rtpSendAACFrame(RtpPacket *rtpPacket, uint8_t *frame, uint32_t frameSize)
{
    //打包文档：https://blog.csdn.net/yangguoyu8023/article/details/106517251/
    int ret;

    rtpPacket->payload[0] = 0x00;
    rtpPacket->payload[1] = 0x10;
    rtpPacket->payload[2] = (frameSize & 0x1FE0) >> 5; //高8位
    rtpPacket->payload[3] = (frameSize & 0x1F) << 3; //低5位

    memcpy(rtpPacket->payload + 4, frame, frameSize);
    ret = m_rtpSocket->writeDatagram((char *)rtpPacket, frameSize + 4,
                                         QHostAddress(clientIP), clientPort);
    if(ret < 0)
        return -1;

    rtpPacket->rtpHeader.seq++;
    /*
     * 如果采样频率是44100
     * 一般AAC每个1024个采样为一帧
     * 所以一秒就有 44100 / 1024 = 43帧
     * 时间增量就是 44100 / 43 = 1025
     * 一帧的时间为 1 / 43 = 23ms
     */
    rtpPacket->rtpHeader.timestamp += 1025;

    return 0;
}
